#!/usr/bin/env python

# reference means origin is back left shoulder (mm)
# actual means origin is arms extended (mm)
# machine means number of steps (1/16 microsteps)

import math,copy,turtle,sys

try:
    import numpy
except:
    print "Download numpy."
    raw_input("PRESS ENTER TO EXIT PROGRAM")
    sys.exit()

try:
    import scipy.optimize
    import scipy.interpolate 
    
except:
    print "Download scipy."
    raw_input("PRESS ENTER TO EXIT PROGRAM")
    sys.exit()

#How many mm does the machine think is one rotation of the x or y axis?
mm_rotation=400    

#various machine coordinate sets where the effector barely touches the bed
touch_points=[(1000,1000,138.8),(1200,700,138.7),(1600,1000,139.2),(1200,1600,141.6),(700,1200,139.8)] 

#CALIBRATION DATA FOR THE BED HEIGHT
z_machine_actual=[(1,0.03),(2,2.1),(3,2.94),(4,3.74),
                  (5,4.8),(6,5.8),(7,6.8),(8,8.7),
                  (9,9.6),(10,10.5),(11,11.7),(12,12.7),
                  (13,13.7),(14,14.7),(15,15.7),(16,16.7),
                  (17,17.7),(18,18.7),(19,19.7),(20,20.7),
                  (21,21.7),(22,22.8),(23,23.9),(24,25.0),
                  (25,26.1),(26,27.2),(27,28.3),(28,29.4),
                  (29,30.5),(30,31.6),(31,32.7),(32,33.8),
                  (33,34.8),(34,35.8),(35,36.8),(36,37.7),
                  (37,38.9),(38,39.9),(39,40.9),(40,42.0),
                  (41,43.59),(42,44.65),(43,45.71),(44,46.79),
                  (45,47.84),(46,48.88),(47,49.56),(48,50.5),
                  (49,51.5),(50,52.6),(51,53.34),(52,54.42),
                  (53,55.48),(54,56.5),(55,57.5),(56,58.48),
                  (57,59.47),(58,60.43),(59,61.41),(60,63.0),
                  (61,64.28),(62,65.32),(63,66.57),(64,67.65),
                  (65,68.75),(66,69.83),(67,70.94),(68,71.01),
                  (69,72.09),(70,73.13),(71,71.15),(72,72.14),
                  (73,73.1),(74,74.07),(75,75.08),(76,76.08),
                  (77,77.13),(78,78.14),(79,79.19),(80,80.25),
                  (81,81.32),(82,82.38),(83,83.44),(84,84.51),
                  (85,85.58),(86,86.66),(87,87.62),(88,88.6),
                  (89,89.54),(90,90.5),(91,91.47),(92,92.45),
                  (93,93.47),(94,94.5),(95,95.55),(96,96.6),
                  (97,97.62),(98,98.65),(99,99.72),(100,100.76),
                  (101,101.83),(102,102.86),(103,103.85),(104,104.85),
                  (105,105.81),(106,106.8),(107,107.72),(108,108.68),
                  (109,109.7),(110,110.7),(111,111.71),(112,112.73),
                  (113,113.75),(114,114.76),(115,115.8),(116,116.84),
                  (117,117.87),(118,118.88),(119,119.87),(120,120.85),
                  (121,121.74),(122,122.73),(123,123.63),(124,124.54),
                  (125,125.49),(126,126.46),(127,127.42),(128,128.4),
                  (129,129.39),(130,130.39),(131,131.5),(141,141.0),(145,145.0)]

#the z height where the bed arms are at 90 degrees
square_z=75.0

#LENGTH OF ARMS
l=150

#DISTANCE BETWEEN SHOULDERS
L=250

#DISTANCE FROM BED ARM ATTACHMENT TO THE CENTER OF THE BED IN THE Y DIRECTION
y_offset=37.5

#Using "G1 X? Y?" to find the machine coordinates that make the arms colinear
straight_forearms=1051


machine_z=numpy.array([i for i,j in z_machine_actual])
actual_z=numpy.array([j for i,j in z_machine_actual])
square_z=float(square_z)
l=float(l)
L=float(L)
y_offset=float(y_offset)
straight_forearms=float(straight_forearms)
mm_rotation=float(mm_rotation)
mechanical_advantage=(straight_forearms/(mm_rotation/2)*math.pi+math.asin(1-L/2.0/l)+math.asin(L/4.0/l))/(math.pi-math.acos(1-L/2.0/l))




def interpolate2(v,leftLookup=True):
    try:
        x=machine_z
        y=actual_z
        if leftLookup:
            f = scipy.interpolate.interp1d(x, y)#, kind='cubic')
        else:
            f = scipy.interpolate.interp1d(y,x)
        return float(f(v))
    except:
        return 0

    """total_weight=0
    new_v=0
    table.sort()
    for m,a in table:
        if not leftLookup:
            m,a=a,m
        if m!=v:
            weight=1.0/(m-v)**2
        else:
            weight=100.0
        new_v+=weight*a
        total_weight+=weight
    return new_v/total_weight"""
#print interpolate2(0)



def machine2reference((x,y,z)):
    zprime=interpolate2(z)
    def func((i,j)):
        (x2,y2,z2)=reference2machine((i,j,0))
        return (x-x2)**2+(y-y2)**2
    xprime,yprime=scipy.optimize.fmin(func,(100,-100), xtol=0.000001, ftol=0.000001,disp=False)
    return xprime,yprime,zprime
    
def reference2machine((x,y,z)):
    try:
        zprime=interpolate2(z,False)
        initial_angle=math.acos(L/(4*l))
        left_leg=math.sqrt(x*x+y*y)
        right_leg=math.sqrt((L-x)*(L-x)+y*y)
        left_elbow=math.acos((left_leg*left_leg-2*l*l)/(-2*l*l))
        right_elbow=math.acos((right_leg*right_leg-2*l*l)/(-2*l*l))
        left_small_angle=(math.pi-left_elbow)/2
        right_small_angle=(math.pi-right_elbow)/2
        left_virtual=math.atan(-y/x)
        right_virtual=math.atan(-y/(L-x))
        left_drive=left_small_angle+left_virtual-initial_angle
        right_drive=right_small_angle+right_virtual-initial_angle
        left_stepper=-left_drive+(math.pi-left_elbow)*mechanical_advantage
        right_stepper=-right_drive+(math.pi-right_elbow)*mechanical_advantage
        return left_stepper*mm_rotation/2/math.pi,right_stepper*mm_rotation/2/math.pi,zprime
    except:
        return 0,0,0

def refPlane():
    ref_points=[(machine2reference(p)) for p in touch_points]
    #print ref_points
    def func((a,b,c,d)):
        v=0
        for x,y,z in ref_points:
            v+=(a*x+b*y+c*z+d)**2
        return v
    a,b,c,d=scipy.optimize.fmin(func,(1,1,1,1),disp=False)
    return a,b,c,d

#print "Finding bed level from touch points.  This may take a while."
ap,bp,cp,dp=refPlane()
#print ap,bp,cp,dp

#print machine2reference((1000,1000,100))
#print reference2machine((125,125,100))
def actual2reference((x,y,z)):
    bed_angle=math.asin((z-interpolate2(square_z))/l)
    leg_offset=l*math.cos(bed_angle)
    yprime=y+y_offset-leg_offset
    xprime=x+L/2
    zero_z=(-dp-ap*xprime-bp*yprime)/cp
    zprime=zero_z-z
    return xprime,yprime,zprime
#print actual2reference((0,0,0))

def reference2actual((x,y,z)):
    pass

def transform(x,y,z):
    return reference2machine(actual2reference((x,y,z)))
#print transform(0,0,0)

def testcode(x,y,z):
    a,b,c=transform(x,y,z)
    #print transform(x,y,z)
    return "G1 X"+str(a)+" Y"+str(b)+" Z"+str(c)+" F9000"
#print testcode(0,0,0)

def getABC(position1):
    global coord
    if "X" not in position1:
        return position1
    position=copy.deepcopy(position1)
    d=distance(coord,position)
    f=position["F"]
    a1,b1,c1=transform(coord["X"],coord["Y"],coord["Z"])
    a2,b2,c2=transform(position["X"],position["Y"],position["Z"])                                                     
    virtual_d=math.sqrt((a1-a2)**2+(b1-b2)**2+(c1-c2)**2)
    fnew=f*1.0
    if d!=0:
        fnew=f*virtual_d/d

    position['X']=a2
    position['Y']=b2
    position['Z']=c2
    position['F']=fnew
    coord=position1
    return position
#print testcode(0,0,0)


def distance(start, end):
    try:
        x1,y1,z1=start['X'],start['Y'],start['Z']
        x2,y2,z2=end['X'],end['Y'],end['Z']
        return math.sqrt((x1-x2)**2+(y1-y2)**2+(z1-z2)**2)
    except:
        return 0
#print testcode(0,0,0)


def interpolate(start, end, i, n):
    x1,y1,z1,e1=start['X'],start['Y'],start['Z'],start['E']
    x2,y2,z2,e2=end['X'],end['Y'],end['Z'],end['E']
    middle={}
    for c in end:
        if c in end and c in start and c in "XYZE":
            middle[c]=(i*end[c]+(n-i)*start[c])/float(n)
        else:
            middle[c]=end[c]
    return middle


def segmentize(start,end,maxLength):
    l=distance(start,end)
    if l<=maxLength:
        return [end]
    else:
        output=[]
        n=int(math.ceil(l/maxLength))
        for i in range(1,n+1):
            output.append(interpolate(start,end,i,n))
        return output

f=sys.stdin
coord={"X":0,"Y":0,"Z":0, "E":0, "F":0}
prefixes="MGXYZESF"
commands="MG"
f2=file(sys.argv[1], "w")
f2.write("G92 X0 Y0 Z0 E0\n")
program=[]
move_count=0
for line in f:
    line=line.strip()
    chunks=line.split(";")[0].split(" ")
    stuff={}
    for chunk in chunks:
        if len(chunk)>1:
            stuff[chunk[0]]=chunk[1:]
            try:
                stuff[chunk[0]]=int(stuff[chunk[0]])
            except:
                try:
                    stuff[chunk[0]]=float(stuff[chunk[0]])
                except:
                    pass
        if "X" in stuff or "Y" in stuff or "Z" in stuff:
            move_count+=1
            for c in coord:
                if c not in stuff:
                    stuff[c]=coord[c]           
    if move_count<=3 and len(stuff)>0:
        program+=[stuff]
    elif len(stuff)>0:
        segments=segmentize(coord,stuff,1)
        program+=segments
    for c in coord:
        if c in stuff:
            coord[c]=stuff[c]
for i in range(len(program)):
    line=program[i]
    if i%100==0:
        print str(i*100.0/len(program))+"%"
    abcline=getABC(line)
    for letter in prefixes:
        if letter in abcline and letter in commands:
            f2.write(letter+str(abcline[letter])+" ")
        elif letter in abcline:
            f2.write(letter+str(round(abcline[letter],3))+" ")
    f2.write("\n")
f2.close()
